/*
 * @Author: EltonLi lijiaming@uavi-tech.com
 * @Date: 2023-09-12 17:48:39
 * @LastEditors: LVGRAPE
 * @LastEditTime: 2025-05-28 17:49:56
 * @FilePath: \zino-fc-v4\applications\ZINO_FC_V4.0\statecontrol\statecontrol.c
 * @Description: 这是默认设置,请设置`customMade`, 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE
 */
#include <rtthread.h>
#include "statecontrol.h"
#include "position.h"
#include "attitude.h"
#include "statecal.h"
#include "usercmd.h"
#include "zino.h"
#include "shanwai.h"
#include "fc_error.h"
#include "motorset.h"
#include "motorset.h"

#define DBG_TAG "statecontrol"
#define DBG_LVL DBG_LOG
#include <rtdbg.h>

// yaw角转动速度
#define YAW_RATE 1.0f

// attitude_t attitudeExpect;
attitude_t attitudeRateExpect;
fc_control_t fc_control;

void statecontrol_init(void)
{
    // rt_kprintf("pid[PID_ANGLE_ROLL] = %d  %d  %d\n", pidSave()->pid[PID_ANGLE_ROLL].P, pidSave()->pid[PID_ANGLE_ROLL].I, pidSave()->pid[PID_ANGLE_ROLL].D);
    // rt_kprintf("pid[PID_RATE_ROLL] = %d  %d  %d\n", pidSave()->pid[PID_RATE_ROLL].P, pidSave()->pid[PID_RATE_ROLL].I, pidSave()->pid[PID_RATE_ROLL].D);
    attitudePidInit(0.001f);
    positionPidInit(0.001f);
}
void position_control(fc_control_t *out, setPoint_t *set, fc_sensor_data_t *sensor, zdrone_state_t *state, float dt)
{
    if (FC_ERROR_CHECK(FC_IMU_CALIBRATING) || (!FC_ERROR_CHECK(FC_ARMED))|| FC_ERROR_CHECK(FC_FLIPING_MODE) || FC_ERROR_CHECK(FC_MANUAL_MODE))
    {
        return;
    }
    positionPidUpdate(set, state, &out->thrust, dt);
}
void statecontrol(fc_control_t *out, setPoint_t *set, fc_sensor_data_t *sensor, zdrone_state_t *state, float dt)
{
    // 检查是否处于IMU校准状态或未解锁状态，如果是，则重置所有PID，并将输出和设定值设为0
    if (FC_ERROR_CHECK(FC_IMU_CALIBRATING) || !FC_ERROR_CHECK(FC_ARMED))
    {
        // attitudeResetAllPID();
        pidResetAll();
        out->pitch = 0;
        out->roll = 0;
        out->thrust = 0;
        out->yaw = 0;
        set->Angle.pitch = 0;
        set->Angle.roll = 0;
        set->Angle.yaw = state->attitude.yaw;
        set->AngleRate.pitch = 0;
        set->AngleRate.roll = 0;
        set->AngleRate.yaw = 0;
        set->Vel.x = 0;
        set->Vel.y = 0;
        set->Vel.z = 0;
        set->Pos.x = state->pos.x;
        set->Pos.y = state->pos.y;
        set->Pos.z = state->pos.z;
        return;
    }



    if(FC_ERROR_CHECK(FC_FLIPING_MODE))
    {

    }
    else
    {
        if (set->Angle.yaw > 180.0f)
            set->Angle.yaw -= 360.0f;
        if (set->Angle.yaw < -180.0f)
            set->Angle.yaw += 360.0f;
        attitudeAnglePID(&set->Angle, state, &set->AngleRate);
    }

    attitudeRatePID(&set->AngleRate, out, state);
}

void statecontrol_task(void *p)
{
    static bool pidInit = false;
    if (!pidInit)
    {
        rt_thread_mdelay(1);
        statecontrol_init();
        fc_motor_on();
        pidInit = true;
    }
    if (FC_ERROR_CHECK(FC_IMU_CALIBRATING))
        return;
    while (1)
    {
        rt_thread_mdelay(10);
    }
}

int statecontrol_task_entry(void)
{
    rt_thread_t statecontrol_update_thread = rt_thread_create("statecontrol_task", statecontrol_task, RT_NULL, 512, 10, 10);
    if (statecontrol_update_thread != RT_NULL)
    {
        rt_thread_startup(statecontrol_update_thread);
    }
    else
    {
        LOG_E("statecontrol_update_thread create failed.");
    }
    return 0;
}

// ZINO_APP_EXPORT(statecontrol_task_entry);
